﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Threading;
using grounding.nao;
using grounding.nao.sensors;

namespace SensorDataVisualizer
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        Nao nao;

        public MainWindow()
        {
            InitializeComponent();
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            // Connect to NAO.
            nao = new Nao("128.208.4.19", 9559);

            nao.SensorFrameReceived += new EventHandler<NaoSensorFrame>(nao_SensorFrameReceived);
        }

        void nao_SensorFrameReceived(object sender, NaoSensorFrame e)
        {
            // Continue updating visualization.
            NaoSensorFrame sensorData = (NaoSensorFrame)nao.GetSensorFrame();

            Dispatcher.BeginInvoke(new Action(
                delegate()
                {
                    // Update sonar bar.
                    SonarSensorFrame sonarFrame = (SonarSensorFrame)sensorData["sonar"];
                    double[] sonarData = sonarFrame.ToArray();

                    sonar_fill.Width = Math.Min(Math.Max((sonarData[0] / 2.55) * sonar_box.Width, 0), sonar_box.Width);

                    // Update foot pressure bars.
                    FootSensorFrame footFrame = (FootSensorFrame)sensorData["foot"];
                    double[] footData = footFrame.ToArray();

                    lfoot_fl_fill.Width = Math.Min(Math.Max((footData[0] / 1.5) * lfoot_fl_box.Width, 0), lfoot_fl_box.Width);
                    lfoot_fr_fill.Width = Math.Min(Math.Max((footData[1] / 1.5) * lfoot_fr_box.Width, 0), lfoot_fr_box.Width);
                    lfoot_rl_fill.Width = Math.Min(Math.Max((footData[2] / 1.5) * lfoot_rl_box.Width, 0), lfoot_rl_box.Width);
                    lfoot_rr_fill.Width = Math.Min(Math.Max((footData[3] / 1.5) * lfoot_rr_box.Width, 0), lfoot_rr_box.Width);
                    rfoot_fl_fill.Width = Math.Min(Math.Max((footData[4] / 1.5) * rfoot_fl_box.Width, 0), rfoot_fl_box.Width);
                    rfoot_fr_fill.Width = Math.Min(Math.Max((footData[5] / 1.5) * rfoot_fr_box.Width, 0), rfoot_fr_box.Width);
                    rfoot_rl_fill.Width = Math.Min(Math.Max((footData[6] / 1.5) * rfoot_rl_box.Width, 0), rfoot_rl_box.Width);
                    rfoot_rr_fill.Width = Math.Min(Math.Max((footData[7] / 1.5) * rfoot_rr_box.Width, 0), rfoot_rr_box.Width);

                    // Update accelerometer bars.
                    AccelerometerFrame accelFrame = (AccelerometerFrame)sensorData["acceleration"];
                    double[] accelData = accelFrame.ToArray();

                    double accel_x_half = accel_box_x.Width / 2;
                    double accel_y_half = accel_box_y.Width / 2;
                    double accel_z_half = accel_box_z.Width / 2;

                    double accel_x = accel_box_x.Margin.Left;
                    double accel_y = accel_box_y.Margin.Left;
                    double accel_z = accel_box_z.Margin.Left;

                    accel_fill_x.Width = Math.Min(Math.Max((Math.Abs(accelData[0]) / 100) * accel_x_half, 0), accel_x_half);
                    accel_fill_y.Width = Math.Min(Math.Max((Math.Abs(accelData[1]) / 100) * accel_y_half, 0), accel_y_half);
                    accel_fill_z.Width = Math.Min(Math.Max((Math.Abs(accelData[2]) / 100) * accel_z_half, 0), accel_z_half);

                    Thickness accel_x_margin = accel_fill_x.Margin;
                    if (accelData[0] < 0)
                        accel_x_margin.Left = accel_x + accel_x_half - accel_fill_x.Width;
                    else
                        accel_x_margin.Left = accel_x + accel_x_half;
                    accel_fill_x.Margin = accel_x_margin;

                    Thickness accel_y_margin = accel_fill_y.Margin;
                    if (accelData[1] < 0)
                        accel_y_margin.Left = accel_y + accel_y_half - accel_fill_y.Width;
                    else
                        accel_y_margin.Left = accel_y + accel_y_half;
                    accel_fill_y.Margin = accel_y_margin;

                    Thickness accel_z_margin = accel_fill_z.Margin;
                    if (accelData[2] < 0)
                        accel_z_margin.Left = accel_z + accel_z_half - accel_fill_z.Width;
                    else
                        accel_z_margin.Left = accel_z + accel_z_half;
                    accel_fill_z.Margin = accel_z_margin;

                    // Update electrical current bars.
                    JointCurrentSensorFrame jointCurrentFrame = (JointCurrentSensorFrame)sensorData["joint_current"];
                    double[] currentData = jointCurrentFrame.ToArray();

                    const double currentCap = 1.3;
                    int c = 0;
                    HeadPitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * HeadPitch_box.Width, 0), HeadPitch_box.Width);
                    HeadYaw_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * HeadYaw_box.Width, 0), HeadYaw_box.Width);
                    LAnklePitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LAnklePitch_box.Width, 0), LAnklePitch_box.Width);
                    LAnkleRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LAnkleRoll_box.Width, 0), LAnkleRoll_box.Width);
                    LElbowRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LElbowRoll_box.Width, 0), LElbowRoll_box.Width);
                    LElbowYaw_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LElbowYaw_box.Width, 0), LElbowYaw_box.Width);
                    LHand_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LHand_box.Width, 0), LHand_box.Width);
                    LHipPitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LHipPitch_box.Width, 0), LHipPitch_box.Width);
                    LHipRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LHipRoll_box.Width, 0), LHipRoll_box.Width);
                    LHipYawPitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LHipYawPitch_box.Width, 0), LHipYawPitch_box.Width);
                    LKneePitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LKneePitch_box.Width, 0), LKneePitch_box.Width);
                    LShoulderPitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LShoulderPitch_box.Width, 0), LShoulderPitch_box.Width);
                    LShoulderRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LShoulderRoll_box.Width, 0), LShoulderRoll_box.Width);
                    LWristYaw_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * LWristYaw_box.Width, 0), LWristYaw_box.Width);
                    RAnklePitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RAnklePitch_box.Width, 0), RAnklePitch_box.Width);
                    RAnkleRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RAnkleRoll_box.Width, 0), RAnkleRoll_box.Width);
                    RElbowRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RElbowRoll_box.Width, 0), RElbowRoll_box.Width);
                    RElbowYaw_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RElbowYaw_box.Width, 0), RElbowYaw_box.Width);
                    RHand_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RHand_box.Width, 0), RHand_box.Width);
                    RHipPitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RHipPitch_box.Width, 0), RHipPitch_box.Width);
                    RHipRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RHipRoll_box.Width, 0), RHipRoll_box.Width);
                    RKneePitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RKneePitch_box.Width, 0), RKneePitch_box.Width);
                    RShoulderPitch_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RShoulderPitch_box.Width, 0), RShoulderPitch_box.Width);
                    RShoulderRoll_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RShoulderRoll_box.Width, 0), RShoulderRoll_box.Width);
                    RWristYaw_fill.Width = Math.Min(Math.Max((currentData[c++] / currentCap) * RWristYaw_box.Width, 0), RWristYaw_box.Width);

                    // Update temperature bar.
                    TemperatureSensorFrame tempFrame = (TemperatureSensorFrame)sensorData["temperature"];
                    double[] tempData = tempFrame.ToArray();

                    double hot = tempData.Average();

                    temp_fill.Width = Math.Min(Math.Max((tempData.Average() / 50) * temp_box.Width, 0), temp_box.Width);

                }));
        }

        private void Window_Closed(object sender, EventArgs e)
        {
            //nao.Disconnect();
        }
    }
}
